/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2013, SRI International
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of SRI International nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Sachin Chitta, Dave Coleman, Mike Lautman */

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <moveit_msgs/msg/display_robot_state.h>
#include <moveit_msgs/msg/display_trajectory.h>

#include <moveit_msgs/msg/attached_collision_object.h>
#include <moveit_msgs/msg/collision_object.h>

#include <moveit_visual_tools/moveit_visual_tools.h>

// 所有使用 ROS 日志记录的源文件都应在文件顶部定义一个文件特定的静态常量 rclcpp::Logger，
// 并且如果存在作用域最窄的命名空间，则应放置在其中
static const rclcpp::Logger LOGGER = rclcpp::get_logger("move_group_demo");

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions node_options;
  node_options.automatically_declare_parameters_from_overrides(true);
  auto move_group_node = rclcpp::Node::make_shared("move_group_interface_tutorial", node_options);

  // 我们为当前状态监视器启动一个单线程执行器，以获取有关机器人状态的信息。
  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(move_group_node);
  std::thread([&executor]() { executor.spin(); }).detach();

  // 开始教程
  //
  // 设置
  // ^^^^^
  //
  // MoveIt 操作一组称为 "规划组" (planning groups) 的关节，并将它们存储在一个称为 
  // ``JointModelGroup`` 的对象中。在 MoveIt 中，术语 "规划组planning group" 和 "关节模型组joint_model_group" 是可以互换使用的。
  static const std::string PLANNING_GROUP = "zc4";

  // MoveGroupInterface 类可以仅使用你想要控制和规划的规划组的名称进行轻松设置。
  moveit::planning_interface::MoveGroupInterface move_group(move_group_node, PLANNING_GROUP);

  // 我们将使用 PlanningSceneInterface 类在我们的 "虚拟世界" 场景中添加和移除碰撞对象
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

  // 原始指针经常用于引用规划组，以提高性能。
  const moveit::core::JointModelGroup* joint_model_group =
      move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

  // 可视化
  // ^^^^^^^^^^^^^
  namespace rvt = rviz_visual_tools;
  moveit_visual_tools::MoveItVisualTools visual_tools(move_group_node, "base_link", "move_group_tutorial",
                                                      move_group.getRobotModel());

  visual_tools.deleteAllMarkers();

  /* 远程控制是一个自省工具，允许用户通过 RViz 中的按钮和键盘快捷键逐步执行高级脚本 */
  visual_tools.loadRemoteControl();

  // RViz 提供多种类型的标记，在本演示中，我们将使用文本、圆柱体和球体
  Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
  text_pose.translation().z() = 1.0;
  visual_tools.publishText(text_pose, "MoveGroupInterface_Demo", rvt::WHITE, rvt::XLARGE);

  // 批量发布用于减少发送到 RViz 的消息数量，以提高可视化效率
  visual_tools.trigger();

  // 获取基本信息
  // ^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // 我们可以打印此机器人的参考坐标系名称。
  RCLCPP_INFO(LOGGER, "Planning frame: %s", move_group.getPlanningFrame().c_str());

  // 我们还可以打印该组的末端执行器链接的名称。
  RCLCPP_INFO(LOGGER, "End effector link: %s", move_group.getEndEffectorLink().c_str());

  // 我们可以获取机器人中的所有规划组的列表：
  RCLCPP_INFO(LOGGER, "Available Planning Groups:");
  std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
            std::ostream_iterator<std::string>(std::cout, ", "));

  // 开始演示
  // ^^^^^^^^^^^^^^^^^^^^^^^^^
  visual_tools.prompt("在 RvizVisualToolsGui 窗口中按 'next' 按钮以开始演示");

  // 规划到位姿目标
  // ^^^^^^^^^^^^^^^^^^^^^^^
  // 我们可以为该组规划一个运动，使末端执行器达到目标位姿。
  geometry_msgs::msg::Pose target_pose1;
  target_pose1.orientation.w = 1.0;
  target_pose1.position.x = 0.3;
  target_pose1.position.y = 0.0;
  target_pose1.position.z = 0.25;
  move_group.setPoseTarget(target_pose1);

  // 现在，我们调用规划器计算规划路径并进行可视化。
  // 请注意，我们只是进行规划，而不是要求 move_group 实际移动机器人。
  moveit::planning_interface::MoveGroupInterface::Plan my_plan;

  bool success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);

  RCLCPP_INFO(LOGGER, "可视化规划 1 (位姿目标) %s", success ? "" : "失败");

  // 可视化规划路径
  // ^^^^^^^^^^^^^^^^^
  // 我们还可以在 RViz 中将规划路径可视化为一条轨迹线。
  RCLCPP_INFO(LOGGER, "将规划 1 可视化为轨迹线");
  visual_tools.publishAxisLabeled(target_pose1, "pose1");
  visual_tools.publishText(text_pose, "Pose_Goal", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  visual_tools.trigger();
  visual_tools.prompt("在 RvizVisualToolsGui 窗口中按 'next' 按钮继续演示");



  // 移动到位姿目标
  // ^^^^^^^^^^^^^^^^^^^^^
  //
  // 移动到位姿目标与上一步类似，
  // 但我们现在使用 `move()` 函数。请注意，
  // 之前设置的位姿目标仍然是活动的，
  // 因此机器人将尝试移动到该目标。
  // 在本教程中，我们不会使用该函数，
  // 因为它是一个阻塞函数，并且需要一个活动的控制器
  // 来执行轨迹并报告执行成功。

  /* 在使用真实机器人时取消注释以下行 */
  /* move_group.move(); */

  // 规划到关节空间目标
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // 让我们设置一个关节空间目标并向其移动。
  // 这将替换之前设置的位姿目标。
  //
  // 首先，我们创建一个指针，引用当前机器人的状态。
  // `RobotState` 对象包含所有当前的位置信息、速度和加速度数据。
  moveit::core::RobotStatePtr current_state = move_group.getCurrentState(10);
  //
  // 接下来，获取当前规划组的关节值。
  std::vector<double> joint_group_positions;
  current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);

  // 现在，我们修改其中一个关节的值，规划到新的关节空间目标，并可视化规划。
  joint_group_positions[0] = 1.0;  // 弧度
  bool within_bounds = move_group.setJointValueTarget(joint_group_positions);
  if (!within_bounds)
  {
    RCLCPP_WARN(LOGGER, "目标关节位置超出限制，但我们仍会进行规划，并将其限制在允许范围内");
  }

  // 我们将允许的最大速度和加速度降低到最大值的 5%。
  // 默认值为 10%（0.1）。
  // 你可以在机器人 `moveit_config` 的 `joint_limits.yaml` 文件中设置默认值，
  // 或者在代码中显式设置因子，以调整机器人的移动速度。
  move_group.setMaxVelocityScalingFactor(0.05);
  move_group.setMaxAccelerationScalingFactor(0.05);

  success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);
  RCLCPP_INFO(LOGGER, "可视化规划 2（关节空间目标）%s", success ? "" : "失败");

  // 在 RViz 中可视化规划：
  visual_tools.deleteAllMarkers();
  visual_tools.publishText(text_pose, "Joint_Space_Goal", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  visual_tools.trigger();
  visual_tools.prompt("在 RvizVisualToolsGui 窗口中按 'next' 按钮继续演示");

  // 规划带有路径约束的运动
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // 可以为机器人上的某个链接轻松指定路径约束。
  // 让我们为组设置一个路径约束和一个位姿目标。
  // 首先定义路径约束。
  moveit_msgs::msg::OrientationConstraint ocm;
  ocm.link_name = "end";
  ocm.header.frame_id = "base_link";
  ocm.orientation.w = 1.0;
  ocm.absolute_x_axis_tolerance = 0.1;
  ocm.absolute_y_axis_tolerance = 0.1;
  ocm.absolute_z_axis_tolerance = 0.1;
  ocm.weight = 1.0;

  // 现在，将其设置为该组的路径约束。
  moveit_msgs::msg::Constraints test_constraints;
  test_constraints.orientation_constraints.push_back(ocm);
  move_group.setPathConstraints(test_constraints);

  // 强制在关节空间中规划
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // 根据规划问题的不同，MoveIt 在 `关节空间` 和 `笛卡尔空间` 之间选择问题表示方式。
  // 在 `ompl_planning.yaml` 文件中设置 `enforce_joint_model_state_space:true` 参数，
  // 可以强制使用 `关节空间` 进行所有规划。
  //
  // 默认情况下，带有方向路径约束的规划请求会在 `笛卡尔空间` 进行采样，
  // 以便调用逆运动学（IK）求解器进行生成采样。
  //
  // 通过强制使用 `关节空间`，规划过程将使用拒绝采样法找到有效的请求。
  // 需要注意的是，这可能会显著增加规划时间。
  //
  // 我们将重新使用之前的目标，并进行规划。
  // 请注意，这仅在当前状态已经满足路径约束时才会起作用。
  // 因此，我们需要将起始状态设置为新的位姿。
  moveit::core::RobotState start_state(*move_group.getCurrentState());
  geometry_msgs::msg::Pose start_pose2;
  start_pose2.orientation.w = 1.0;
  start_pose2.position.x = 0.04;
  start_pose2.position.y = -0.;
  start_pose2.position.z = 0.32;
  start_state.setFromIK(joint_model_group, start_pose2);
  move_group.setStartState(start_state);

  // 现在，我们将从新创建的起始状态规划到之前的位姿目标。
  move_group.setPoseTarget(target_pose1);

  // 由于规划带有约束可能会很慢，因为每个采样都需要调用逆运动学求解器。
  // 让我们将规划时间从默认的 5 秒增加，以确保规划器有足够的时间成功完成规划。
  move_group.setPlanningTime(10.0);

  success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);
  RCLCPP_INFO(LOGGER, "可视化规划 3（带约束）%s", success ? "" : "失败");

  // 在 RViz 中可视化规划：
  visual_tools.deleteAllMarkers();
  visual_tools.publishAxisLabeled(start_pose2, "start");
  visual_tools.publishAxisLabeled(target_pose1, "goal");
  visual_tools.publishText(text_pose, "Constrained_Goal", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  visual_tools.trigger();
  visual_tools.prompt("在 RvizVisualToolsGui 窗口中按 'next' 按钮继续演示");

  // 使用完路径约束后，确保清除它。
  move_group.clearPathConstraints();

  // 笛卡尔路径
  // ^^^^^^^^^^^^^^^
  // 你可以直接规划一条笛卡尔路径，
  // 通过指定一系列末端执行器要经过的路径点。
  // 请注意，我们是从上面的新起始状态开始。
  // 初始位姿（起始状态）不需要添加到路径点列表中，
  // 但添加它可以帮助可视化路径。
  std::vector<geometry_msgs::msg::Pose> waypoints;
  waypoints.push_back(start_pose2);

  geometry_msgs::msg::Pose target_pose3 = start_pose2;

  target_pose3.position.z -= 0.2;
  waypoints.push_back(target_pose3);  // 向下移动

  target_pose3.position.y -= 0.2;
  waypoints.push_back(target_pose3);  // 向右移动

  target_pose3.position.z += 0.2;
  target_pose3.position.y += 0.2;
  target_pose3.position.x -= 0.2;
  waypoints.push_back(target_pose3);  // 向上和向左移动


  // 我们希望笛卡尔路径以 1 cm 的分辨率进行插值，
  // 因此我们将最大步长设置为 0.01，以限制笛卡尔平移的步长。
  const double eef_step = 0.01;
  moveit_msgs::msg::RobotTrajectory trajectory;
  double fraction = move_group.computeCartesianPath(waypoints, eef_step, 0.0, trajectory);
  RCLCPP_INFO(LOGGER, "可视化规划 4（笛卡尔路径）（%.2f%% 完成）", fraction * 100.0);

  // 在 RViz 中可视化规划
  visual_tools.deleteAllMarkers();
  visual_tools.publishText(text_pose, "Cartesian_Path", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL);
  for (std::size_t i = 0; i < waypoints.size(); ++i)
    visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL);
  visual_tools.trigger();
  visual_tools.prompt("在 RvizVisualToolsGui 窗口中按 'next' 按钮继续演示");

  // 笛卡尔运动通常需要较慢的速度，例如在接近物体时。
  // 目前，笛卡尔路径的速度无法通过 `maxVelocityScalingFactor` 进行设置，
  // 需要手动调整轨迹的时间，如此处所述：
  // `here <https://groups.google.com/forum/#!topic/moveit-users/MOoFxy2exT4>`_。
  // 我们欢迎相关的 Pull Request。
  //
  // 你可以像这样执行轨迹：
  /* move_group.execute(trajectory); */

  // 向环境添加对象
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // 首先，让我们规划到另一个简单的目标，该路径上没有障碍物。
  move_group.setStartState(*move_group.getCurrentState());
  geometry_msgs::msg::Pose another_pose;
  another_pose.orientation.w = 0;
  another_pose.orientation.x = -1.0;
  another_pose.position.x = 0.7;
  another_pose.position.y = 0.0;
  another_pose.position.z = 0.59;
  move_group.setPoseTarget(another_pose);

  success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);
  RCLCPP_INFO(LOGGER, "可视化规划 5（无障碍物）%s", success ? "" : "失败");

  visual_tools.deleteAllMarkers();
  visual_tools.publishText(text_pose, "Clear_Goal", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishAxisLabeled(another_pose, "goal");
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  visual_tools.trigger();
  visual_tools.prompt("在 RvizVisualToolsGui 窗口中按 'next' 按钮继续演示");

  // // 结果可能如下所示：
  // //
  // // .. image:: ./move_group_interface_tutorial_clear_path.gif
  // //    :alt: 显示机械臂相对直线移动到目标的动画
  // //
  // // 现在，让我们定义一个碰撞对象 ROS 消息，以便机器人避开它。
  // moveit_msgs::msg::CollisionObject collision_object;
  // collision_object.header.frame_id = move_group.getPlanningFrame();

  // // 该对象的 ID 用于标识它。
  // collision_object.id = "box1";

  // // 定义要添加到世界中的立方体。
  // shape_msgs::msg::SolidPrimitive primitive;
  // primitive.type = primitive.BOX;
  // primitive.dimensions.resize(3);
  // primitive.dimensions[primitive.BOX_X] = 0.1;
  // primitive.dimensions[primitive.BOX_Y] = 1.5;
  // primitive.dimensions[primitive.BOX_Z] = 0.5;

  // // 定义立方体的位姿（相对于 frame_id 设定）。
  // geometry_msgs::msg::Pose box_pose;
  // box_pose.orientation.w = 1.0;
  // box_pose.position.x = 0.48;
  // box_pose.position.y = 0.0;
  // box_pose.position.z = 0.25;

  // collision_object.primitives.push_back(primitive);
  // collision_object.primitive_poses.push_back(box_pose);
  // collision_object.operation = collision_object.ADD;

  // std::vector<moveit_msgs::msg::CollisionObject> collision_objects;
  // collision_objects.push_back(collision_object);

  // // 现在，我们将碰撞对象添加到世界中
  // // （使用一个可以包含多个对象的 vector）
  // RCLCPP_INFO(LOGGER, "向世界中添加一个物体");
  // planning_scene_interface.addCollisionObjects(collision_objects);

  // // 在 RViz 中显示状态文本，并等待 MoveGroup 接收和处理碰撞对象消息
  // visual_tools.publishText(text_pose, "Add_object", rvt::WHITE, rvt::XLARGE);
  // visual_tools.trigger();
  // visual_tools.prompt("在 RvizVisualToolsGui 窗口中按 'next' 按钮，当碰撞对象出现在 RViz 中时继续");

  // // 现在，当我们规划轨迹时，它将避开障碍物。
  // success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);
  // RCLCPP_INFO(LOGGER, "可视化规划 6（位姿目标绕过立方体）%s", success ? "" : "失败");
  // visual_tools.publishText(text_pose, "Obstacle_Goal", rvt::WHITE, rvt::XLARGE);
  // visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  // visual_tools.trigger();
  // visual_tools.prompt("在 RvizVisualToolsGui 窗口中按 'next' 按钮，当规划完成后继续");

  // // 结果可能如下所示：
  // //
  // // .. image:: ./move_group_interface_tutorial_avoid_path.gif
  // //    :alt: 显示机械臂避开新障碍物的动画
  // //
  // // 将对象附加到机器人
  // // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  // //
  // // 你可以将一个对象附加到机器人，使其与机器人几何结构一起移动。
  // // 这模拟了机械臂拾取物体以进行操作的情况。
  // // 运动规划还应避免对象之间的碰撞。

  // // 定义要附加的碰撞对象
  // moveit_msgs::msg::CollisionObject object_to_attach;
  // object_to_attach.id = "cylinder1";

  // shape_msgs::msg::SolidPrimitive cylinder_primitive;
  // cylinder_primitive.type = primitive.CYLINDER;
  // cylinder_primitive.dimensions.resize(2);
  // cylinder_primitive.dimensions[primitive.CYLINDER_HEIGHT] = 0.20;
  // cylinder_primitive.dimensions[primitive.CYLINDER_RADIUS] = 0.04;

  // // 我们为该圆柱体定义框架/位姿，使其出现在机械爪中。
  // object_to_attach.header.frame_id = move_group.getEndEffectorLink();
  // geometry_msgs::msg::Pose grab_pose;
  // grab_pose.orientation.w = 1.0;
  // grab_pose.position.z = 0.2;

  // // 首先，我们将对象添加到世界中（不使用 vector）。
  // object_to_attach.primitives.push_back(cylinder_primitive);
  // object_to_attach.primitive_poses.push_back(grab_pose);
  // object_to_attach.operation = object_to_attach.ADD;
  // planning_scene_interface.applyCollisionObject(object_to_attach);

  // // 然后，我们将对象 "附加" 到机器人。
  // // MoveIt 使用 frame_id 确定它附加到哪个机器人链接。
  // // 我们还需要告诉 MoveIt 该对象允许与机械爪的手指链接发生碰撞。
  // // 你还可以使用 `applyAttachedCollisionObject` 直接将对象附加到机器人。
  // RCLCPP_INFO(LOGGER, "将物体附加到机器人");
  // std::vector<std::string> touch_links;
  // touch_links.push_back("panda_rightfinger");
  // touch_links.push_back("panda_leftfinger");
  // move_group.attachObject(object_to_attach.id, "panda_hand", touch_links);

  // visual_tools.publishText(text_pose, "Object_attached_to_robot", rvt::WHITE, rvt::XLARGE);
  // visual_tools.trigger();

  // /* 等待 MoveGroup 接收并处理附加的碰撞对象消息 */
  // visual_tools.prompt("在 RvizVisualToolsGui 窗口中按 'next' 按钮，当新对象附加到机器人后继续");

  // // 重新规划，但现在手中带有物体。
  // move_group.setStartStateToCurrentState();
  // success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);
  // RCLCPP_INFO(LOGGER, "可视化规划 7（带着圆柱体绕过立方体）%s", success ? "" : "失败");
  // visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  // visual_tools.trigger();
  // visual_tools.prompt("在 RvizVisualToolsGui 窗口中按 'next' 按钮，当规划完成后继续");

  // // 结果可能如下所示：
  // //
  // // .. image:: ./move_group_interface_tutorial_attached_object.gif
  // //    :alt: 显示当对象附加后，机械臂以不同方式移动的动画
  // //
  // // 解除附件并移除对象
  // // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  // //
  // // 现在，让我们从机器人的机械爪中分离圆柱体。
  // RCLCPP_INFO(LOGGER, "从机器人上分离对象");
  // move_group.detachObject(object_to_attach.id);

  // // 在 RViz 中显示状态文本
  // visual_tools.deleteAllMarkers();
  // visual_tools.publishText(text_pose, "Object_detached_from_robot", rvt::WHITE, rvt::XLARGE);
  // visual_tools.trigger();

  // /* 等待 MoveGroup 接收并处理分离的碰撞对象消息 */
  // visual_tools.prompt("在 RvizVisualToolsGui 窗口中按 'next' 按钮，当新对象从机器人上分离后继续");

  // // 现在，让我们从世界中移除对象。
  // RCLCPP_INFO(LOGGER, "从世界中移除对象");
  // std::vector<std::string> object_ids;
  // object_ids.push_back(collision_object.id);
  // object_ids.push_back(object_to_attach.id);
  // planning_scene_interface.removeCollisionObjects(object_ids);

  // // 在 RViz 中显示状态文本
  // visual_tools.publishText(text_pose, "Objects_removed", rvt::WHITE, rvt::XLARGE);
  // visual_tools.trigger();

  // /* 等待 MoveGroup 接收并处理移除的碰撞对象消息 */
  // visual_tools.prompt("在 RvizVisualToolsGui 窗口中按 'next' 按钮，当碰撞对象消失后继续");

  // 结束教程
  visual_tools.deleteAllMarkers();
  visual_tools.trigger();

  rclcpp::shutdown();
  return 0;
}
